using System;
using Microsoft.Xna.Framework;

namespace Oops.Xna.Framework
{
	/// <summary>
	/// Represents a component of a <see cref="Microsoft.Xna.Framework.Vector3" />.
	/// </summary>
	public enum VectorIndex
	{
		/// <summary>
		/// Not the x, y or z-component of a <see cref="Microsoft.Xna.Framework.Vector3" />.
		/// </summary>
		None = -1,

		/// <summary>
		/// The X-component of a <see cref="Microsoft.Xna.Framework.Vector3" />.
		/// </summary>
		X,

		/// <summary>
		/// The Y-component of a <see cref="Microsoft.Xna.Framework.Vector3" />. 
		/// </summary>
		Y,

		/// <summary>
		/// The Z-component of a <see cref="Microsoft.Xna.Framework.Vector3" />.
		/// </summary>
		Z
	}

	/// <summary>
	/// An extension to the <see cref="Microsoft.Xna.Framework.Vector3" /> structure.
	/// </summary>
	public static class Vector3Extension
	{
		/// <summary>
		/// Gets a <see cref="Microsoft.Xna.Framework.Vector3" /> with all components set to <see cref="System.Single.MinValue"/>.
		/// </summary>
		public static Vector3 MinValue = new Vector3(float.MinValue);

		/// <summary>
		/// Gets a <see cref="Microsoft.Xna.Framework.Vector3" /> with all components set to <see cref="System.Single.MaxValue"/>.
		/// </summary>
		public static Vector3 MaxValue = new Vector3(float.MaxValue);

		/// <summary>
		/// Gets the component (i.e., X, Y, Z) at the specified <paramref name="index"/> from the <see cref="Microsoft.Xna.Framework.Vector3" />.
		/// </summary>
		/// <param name="vector">The <see cref="Microsoft.Xna.Framework.Vector3" /> to get the component.</param>
		/// <param name="index">The component to get.</param>
		/// <returns>Returns the specified component's value.</returns>
		public static unsafe float GetComponent(this Vector3 vector, VectorIndex index)
		{
			Vector3* address = &vector;
			return ((float*)address)[(int)index];
		}

		/// <summary>
		/// Gets the component (i.e., X, Y, Z) at the specified <paramref name="index"/> from the <see cref="Microsoft.Xna.Framework.Vector3" />.
		/// </summary>
		/// <param name="vector">The <see cref="Microsoft.Xna.Framework.Vector3" /> to get the component.</param>
		/// <param name="index">The component to get.</param>
		/// <returns>Returns the specified component's value.</returns>
		public static unsafe float GetComponent(ref Vector3 vector, VectorIndex index)
        {
            fixed (Vector3* address = &vector)
            {
                return ((float*)address)[(int)index];
            }
        }

		/// <summary>
		/// Sets the component (i.e., X, Y, Z) at the specified <paramref name="index"/> on the <see cref="Microsoft.Xna.Framework.Vector3" />.
		/// </summary>
		/// <param name="vector">The <see cref="Microsoft.Xna.Framework.Vector3" /> to modify.</param>
		/// <param name="index">The component to modify.</param>
		/// <param name="value">The component's value.</param>
		public static unsafe void SetComponent(ref Vector3 vector, VectorIndex index, float value)
		{
			fixed (Vector3* address = &vector)
			{
				((float*)address)[(int)index] = value;
			}
		}

		/// <summary>
		/// Adds to the component (i.e., X, Y, Z) at the specified <paramref name="index"/> on the <see cref="Microsoft.Xna.Framework.Vector3" />.
		/// </summary>
		/// <param name="vector">The <see cref="Microsoft.Xna.Framework.Vector3" /> to modify.</param>
		/// <param name="index">The component to modify.</param>
		/// <param name="value">The component's value.</param>
		public static unsafe void AddToComponent(ref Vector3 vector, VectorIndex index, float value)
		{
			fixed (Vector3* address = &vector)
			{
				((float*)address)[(int)index] += value;
			}
		}

		/// <summary>
		/// Returns the maximum component of the specified <see cref="Microsoft.Xna,Framework.Vector3"/>.
		/// </summary>
		/// <param name="vector">The source <see cref="Microsoft.Xna.Framework.Vector3"/>.</param>
		/// <returns>The maximum value of the x, y and z components of <paramref name="vector"/>.</returns>
		public static float MaxComponent(this Vector3 vector)
		{
			return Math.Max(vector.X, Math.Max(vector.Y, vector.Z));
		}

		/// <summary>
		/// Returns the maximum component of the specified <see cref="Microsoft.Xna,Framework.Vector3"/>.
		/// </summary>
		/// <param name="vector">The source <see cref="Microsoft.Xna.Framework.Vector3"/>.</param>
		/// <returns>The maximum value of the x, y and z components of <paramref name="vector"/>.</returns>
		public static float MaxComponent(ref Vector3 vector)
		{
			return Math.Max(vector.X, Math.Max(vector.Y, vector.Z));
		}

		/// <summary>
		/// Returns the maximum component of the specified <see cref="Microsoft.Xna,Framework.Vector3"/>.
		/// </summary>
		/// <param name="vector">The source <see cref="Microsoft.Xna.Framework.Vector3"/>.</param>
		/// <param name="result">The maximum value of the x, y and z components of <paramref name="vector"/>.</param>
		public static void MaxComponent(ref Vector3 vector, out float result)
		{
			result = Math.Max(vector.X, Math.Max(vector.Y, vector.Z));
		}

		/// <summary>
		/// Returns the maximum component index of the specified <see cref="Microsoft.Xna,Framework.Vector3"/>.
		/// </summary>
		/// <param name="vector">The source <see cref="Microsoft.Xna.Framework.Vector3"/>.</param>
		/// <returns>The maximum index of the x, y and z components of <paramref name="vector"/>.</returns>
		public static VectorIndex MaxComponentIndex(this Vector3 vector)
		{
			float temp = vector.X;
			VectorIndex result = VectorIndex.X;

			if (vector.X < vector.Y)
			{
				temp = vector.Y;
				result = VectorIndex.Y;
			}

			if (temp < vector.Z)
			{
				result = VectorIndex.Z;
			}

			return result;
		}

		/// <summary>
		/// Returns the maximum component index of the specified <see cref="Microsoft.Xna,Framework.Vector3"/>.
		/// </summary>
		/// <param name="vector">The source <see cref="Microsoft.Xna.Framework.Vector3"/>.</param>
		/// <returns>The maximum index of the x, y and z components of <paramref name="vector"/>.</returns>
		public static VectorIndex MaxComponentIndex(ref Vector3 vector)
		{
			float temp = vector.X;
			VectorIndex result = VectorIndex.X;

			if (vector.X < vector.Y)
			{
				temp = vector.Y;
				result = VectorIndex.Y;
			}

			if (temp < vector.Z)
			{
				result = VectorIndex.Z;
			}

			return result;
		}

		/// <summary>
		/// Returns the maximum component index of the specified <see cref="Microsoft.Xna,Framework.Vector3"/>.
		/// </summary>
		/// <param name="vector">The source <see cref="Microsoft.Xna.Framework.Vector3"/>.</param>
		/// <param name="result">The maximum index of the x, y and z components of <paramref name="vector"/>.</param>
		public static void MaxComponentIndex(ref Vector3 vector, out VectorIndex result)
		{
			float temp = vector.X;
			result = VectorIndex.X;

			if (vector.X < vector.Y)
			{
				temp = vector.Y;
				result = VectorIndex.Y;
			}

			if (temp < vector.Z)
			{
				result = VectorIndex.Z;
			}
		}

		/// <summary>
		/// Rounds each component of the <see cref="Microsoft.Xna,Framework.Vector3"/>.
		/// </summary>
		/// <param name="vector">The source vector.</param>
		/// <param name="increment">The value to round each component to.</param>
		/// <returns>Returns the round vector.</returns>
		public static Vector3 RoundTo(this Vector3 vector, float increment)
		{
			vector.X = MathExtension.RoundTo(vector.X, increment);
			vector.Y = MathExtension.RoundTo(vector.Y, increment);
			vector.Z = MathExtension.RoundTo(vector.Z, increment);

			return vector;
		}

		/// <summary>
		/// Sets the absolute value of the <see cref="Microsoft.Xna.Framework.Vector3"/> components.
		/// </summary>
		/// <param name="vector">The source vector.</param>
		/// <returns>Returns the absolute value of all the components of the source vector.</returns>
		public static Vector3 Abs(this Vector3 vector)
		{
			vector.X = Math.Abs(vector.X);
			vector.Y = Math.Abs(vector.Y);
			vector.Z = Math.Abs(vector.Z);

			return vector;
		}

		/// <summary>
		/// Sets the minimum value of the <see cref="Microsoft.Xna.Framework.Vector3"/> components.
		/// </summary>
		/// <param name="vector">The source vector.</param>
		/// <param name="value">The value for minimum comparision.</param>
		/// <returns>Returns the minimum value of all the components of the source vector.</returns>
		public static Vector3 Min(this Vector3 vector, float value)
		{
			vector.X = Math.Min(vector.X, value);
			vector.Y = Math.Min(vector.Y, value);
			vector.Z = Math.Min(vector.Z, value);

			return vector;
		}

		/// <summary>
		/// Sets the maximum value of the <see cref="Microsoft.Xna.Framework.Vector3"/> components.
		/// </summary>
		/// <param name="vector">The source vector.</param>
		/// <param name="value">The value for maximum comparision.</param>
		/// <returns>Returns the maximum value of all the components of the source vector.</returns>
		public static Vector3 Max(this Vector3 vector, float value)
		{
			vector.X = Math.Max(vector.X, value);
			vector.Y = Math.Max(vector.Y, value);
			vector.Z = Math.Max(vector.Z, value);

			return vector;
		}

		/// <summary>
		/// Creates a unit vector from the specified <see cref="Microsoft.Xna.Framework.Vector3" />.
		/// </summary>
		/// <param name="vector">The source <see cref="Microsoft.Xna.Framework.Vector3" />.</param>
		/// <returns>Returns the created unit vector.</returns>
		public static Vector3 SafeNormalize(Vector3 vector)
		{
			float lengthSquared = vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z;
			if (lengthSquared != 0.0f)
			{
#if FAST_INVSQRT
				float inverseLength = MathExtension.InvSqrt(lengthSquared);
#else
				float inverseLength = 1.0f / (float)Math.Sqrt(lengthSquared);
#endif
				vector.X *= inverseLength;
				vector.Y *= inverseLength;
				vector.Z *= inverseLength;
			}

			return vector;
		}

		/// <summary>
		/// Modifies the specified <see cref="Microsoft.Xna.Framework.Vector3" /> into a unit vector.
		/// </summary>
		/// <param name="vector">The target <see cref="Microsoft.Xna.Framework.Vector3" />.</param>
		public static void SafeNormalize(ref Vector3 vector)
		{
			float lengthSquared = vector.X * vector.X + vector.Y * vector.Y + vector.Z * vector.Z;
			if (lengthSquared != 0.0f)
			{
#if FAST_INVSQRT
				float inverseLength = MathExtension.InvSqrt(lengthSquared);
#else
				float inverseLength = 1.0f / (float)Math.Sqrt(lengthSquared);
#endif
				vector.X *= inverseLength;
				vector.Y *= inverseLength;
				vector.Z *= inverseLength;
			}
		}

		/// <summary>
		/// Determines if the specified vector has a magnitude of zero.
		/// </summary>
		/// <param name="vector">The source vector.</param>
		/// <returns><b>true</b> if the vector is zero length. Otherwise, <b>false</b>.</returns>
		public static bool IsZeroLength(this Vector3 vector)
		{
			return vector.LengthSquared() < (MathExtension.Epsilon * MathExtension.Epsilon);
		}

		/// <summary>
		/// Determines if the specified vector has a magnitude of zero.
		/// </summary>
		/// <param name="vector">The source vector.</param>
		/// <returns><b>true</b> if the vector is zero length. Otherwise, <b>false</b>.</returns>
		public static bool IsZeroLength(ref Vector3 vector)
		{
			return vector.LengthSquared() < (MathExtension.Epsilon * MathExtension.Epsilon);
		}

		/// <summary>
		/// Calculates the dot product of two vectors.
		/// </summary>
		/// <param name="vector1">The first source vector.</param>
		/// <param name="vector2">The second source vector.</param>
		/// <returns>The dot product of <paramref name="vector1"/> and <paramref name="vector2"/>.</returns>
		public static float Dot(ref Vector3 vector1, ref Vector3 vector2)
		{
			return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z;
		}

		/// <summary>
		/// Transforms the specified <see cref="Microsoft.Xna.Framework.Vector3" /> by the transpose of the specified <see cref="Microsoft.Xna.Framework.Matrix" />.
		/// </summary>
		/// <param name="position">The source vector.</param>
		/// <param name="matrix">The transform matrix.</param>
		/// <returns>The transformed vector.</returns>
		public static Vector3 TransformTranspose(Vector3 position, Matrix matrix)
		{
			float x = position.X * matrix.M11 + position.Y * matrix.M12 + position.Z * matrix.M13 + matrix.M14;
			float y = position.X * matrix.M21 + position.Y * matrix.M22 + position.Z * matrix.M23 + matrix.M24;
			float z = position.X * matrix.M31 + position.Y * matrix.M32 + position.Z * matrix.M33 + matrix.M34;

			Vector3 result = new Vector3();
			result.X = x;
			result.Y = y;
			result.Z = z;

			return result;
		}

		/// <summary>
		/// Transforms the specified <see cref="Microsoft.Xna.Framework.Vector3" /> by the transpose of the specified <see cref="Microsoft.Xna.Framework.Matrix" />.
		/// </summary>
		/// <param name="position">The source vector.</param>
		/// <param name="matrix">The transform matrix.</param>
		/// <param name="result">The transform vector.</param>
		public static void TransformTranspose(ref Vector3 position, ref Matrix matrix, out Vector3 result)
		{
			float x = position.X * matrix.M11 + position.Y * matrix.M12 + position.Z * matrix.M13 + matrix.M14;
			float y = position.X * matrix.M21 + position.Y * matrix.M22 + position.Z * matrix.M23 + matrix.M24;
			float z = position.X * matrix.M31 + position.Y * matrix.M32 + position.Z * matrix.M33 + matrix.M34;
#if XBOX
			result = Vector3.Zero;
#endif
			result.X = x;
			result.Y = y;
			result.Z = z;
		}

		/// <summary>
		/// Converts the source <see cref="Vector3"/> to a <see cref="Vector2"/> with no y-component.
		/// </summary>
		/// <param name="source">The source vector.</param>
		/// <returns>
		/// A <see cref="Vector2"/> where <see cref="Vector2.X"/> equals <paramref name="source.X"/>
		/// and <see cref="Vector2.Y"/> equals <paramref name="source.Z"/>.
		/// </returns>
		public static Vector2 ToVector2(this Vector3 source)
		{
			return new Vector2(source.X, source.Z);
		}

		/// <summary>
		/// Converts the source <see cref="Vector3"/> to a <see cref="Vector2"/> with no y-component.
		/// </summary>
		/// <param name="source">The source vector.</param>
		/// <returns>
		/// A <see cref="Vector2"/> where <see cref="Vector2.X"/> equals <paramref name="source.X"/>
		/// and <see cref="Vector2.Y"/> equals <paramref name="source.Z"/>.
		/// </returns>
		public static Vector2 ToVector2(ref Vector3 source)
		{
			return new Vector2(source.X, source.Z);
		}

		/// <summary>
		/// Converts the source <see cref="Vector3"/> to a <see cref="Vector2"/> with no y-component.
		/// </summary>
		/// <param name="source">The source vector.</param>
		/// <param name="result">A <see cref="Vector2"/> where <see cref="result.X"/> equals <paramref name="source.X"/> and <see cref="result.Y"/> equals <paramref name="source.Z"/>.</param>
		public static void ToVector2(ref Vector3 source, out Vector2 result)
		{
			result = new Vector2(source.X, source.Z);
		}

		public static float GetAngleBetween(Vector3 from, Vector3 to, Vector3 toRight)
		{
			Vector3Extension.SafeNormalize(ref from);
			Vector3Extension.SafeNormalize(ref to);
			Vector3Extension.SafeNormalize(ref toRight);

			float forwardDot = Vector3.Dot(from, to);
			float rightDot = Vector3.Dot(from, toRight);

			// Keep dot in range to prevent rounding errors       
			forwardDot = MathHelper.Clamp(forwardDot, -1.0f, 1.0f);
			float angle = (float)Math.Acos((float)forwardDot);

			if (rightDot < 0.0f)
			{
				angle *= -1.0f;
			}

			return angle;
		}
	}
}
